/**
 * Copyright 2021 xpstem.com
 *
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU Lesser General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU Lesser General Public License for more details.
 *
 * You should have received a copy of the GNU Lesser General Public License
 * along with this program.  If not, see <http://www.gnu.org/licenses/>.
 */

package com.xpstem.lelink.operation;

import com.xpstem.lelink.enums.*;
import com.xpstem.lelink.type.Data16;
import com.xpstem.lelink.type.Data32;
import com.xpstem.lelink.type.Data8;
import com.xpstem.lelink.type.EV3Type;

import java.io.ByteArrayOutputStream;
import java.io.IOException;
import java.util.Arrays;
import java.util.List;

/**
 * EV3 output operations.
 * @author billy zhang
 */
public class EV3OutputOpFactory {

    private EV3OutputOpFactory() { }

    /**
     * specifying the output device type
     *
     * @param layer (Data8) LAYER – Specify chain layer number [0 - 3]
     * @param no NO – Port number [0 - 3]
     * @param type TYPE – Output device type, (0x07: Large motor, Medium motor = 0x08)
     */
    public static EV3Operation opOutputSetType(
            Layer layer,
            OutputPort no,
            Data8 type) throws IOException {
        ByteArrayOutputStream outputStream = new ByteArrayOutputStream();

        LCEncoder.from(layer.value()).lc0(outputStream);
        LCEncoder.from(no).lc0(outputStream);
        LCEncoder.from(type).lcx(outputStream);
        byte[] opData = outputStream.toByteArray();

        EV3OperationPackage opPkg = new EV3OperationPackage(OpCode.OUTPUT_SET_TYPE, opData);
        return new EV3Operation(opPkg);
    }


    /**
     * restting the tacho count for the individual output ports
     *
     * @param layer (Data8) LAYER – Specify chain layer number [0 - 3]
     * @param nos (Data8) NOS – Output bit field [0x00 – 0x0F]
     */
    public static EV3Operation opOutputReset(
            Layer layer,
            OutputPort[] nos) throws IOException {
        ByteArrayOutputStream outputStream = new ByteArrayOutputStream();

        LCEncoder.from(layer.value()).lc0(outputStream);
        LCEncoder.from(nos).lc0(outputStream);
        byte[] opData = outputStream.toByteArray();

        EV3OperationPackage opPkg = new EV3OperationPackage(OpCode.OUTPUT_RESET, opData);
        return new EV3Operation(opPkg);
    }

    /**
     * This function enables restting the tacho count for the individual output ports
     *
     * @param layer (Data8) LAYER – Specify chain layer number [0 - 3]
     * @param nos (Data8) NOS – Output bit field [0x00 – 0x0F]
     * @param brake (Data8) BRAKE – Specify break level [0: Float, 1: Break]
     */
    public static EV3Operation opOutputStop(
            Layer layer,
            OutputPort[] nos,
            Brake brake) throws IOException {
        ByteArrayOutputStream outputStream = new ByteArrayOutputStream();

        LCEncoder.from(layer).lc0(outputStream);
        LCEncoder.from(nos).lc0(outputStream);
        LCEncoder.from(brake.value()).lc0(outputStream);
        byte[] opData = outputStream.toByteArray();

        EV3OperationPackage opPackage = new EV3OperationPackage(OpCode.OUTPUT_STOP, opData);
        return new EV3Operation(opPackage);
    }

    /**
     * This function enables setting the output percentage power on the output ports
     *
     * @param layer (Data8) LAYER – Specify chain layer number [0 - 3]
     * @param nos (Data8) NOS – Output bit field [0x00 – 0x0F]
     * @param power (Data8) POWER – Specify output speed [-100 – 100 %]
     */
    public static EV3Operation opOutputPower(
            Layer layer,
            OutputPort[] nos,
            Data8 power) throws IOException {
        ByteArrayOutputStream outputStream = new ByteArrayOutputStream();

        LCEncoder.from(layer).lc0(outputStream);
        LCEncoder.from(nos).lc0(outputStream);
        LCEncoder.from(power).lcx(outputStream);
        byte[] opData = outputStream.toByteArray();

        EV3OperationPackage opPackage = new EV3OperationPackage(OpCode.OUTPUT_POWER, opData);
        return new EV3Operation( opPackage );
    }

    /**
     * This function enables setting the output percentage speed on the output ports. This
     * modes automatically enables speed control, which means the system will automa-
     * tically adjust the power to keep the specified speed.
     *
     * @param layer (Data8) LAYER – Specify chain layer number [0 - 3]
     * @param nos (Data8) NOS – Output bit field [0x00 – 0x0F]
     * @param speed (Data8) SPEED – Specify output speed [-100 – 100 %]
     */
    public static EV3Operation opOutputSpeed(
            Layer layer,
            OutputPort[] nos,
            Data8 speed) throws IOException {
        ByteArrayOutputStream outputStream = new ByteArrayOutputStream();

        LCEncoder.from(layer).lc0(outputStream);
        LCEncoder.from(nos).lc0(outputStream);
        LCEncoder.from(speed).lcx(outputStream);
        byte[] opData = outputStream.toByteArray();

        EV3OperationPackage opPackage = new EV3OperationPackage(OpCode.OUTPUT_SPEED, opData);
        return new EV3Operation(opPackage );
    }

    /**
     * This function enables starting the specified output port.
     *
     * @param layer (Data8) LAYER – Specify chain layer number, [0 - 3]
     * @param nos (Data8) NOS – Output bit field, [0x00 – 0x0F]
     */
    public static EV3Operation opOutputStart(
            Layer layer,
            OutputPort[] nos) throws IOException {
        ByteArrayOutputStream outputStream = new ByteArrayOutputStream();

        LCEncoder.from(layer).lc0(outputStream);
        LCEncoder.from(nos).lc0(outputStream);
        byte[] opData = outputStream.toByteArray();

        EV3OperationPackage opPackage = new EV3OperationPackage(OpCode.OUTPUT_START, opData);
        return new EV3Operation( opPackage );
    }

    /**
     * This function enables starting the specified output port.
     * Polarity parameter:
     * -1 : Motor will run backward
     * 0 : Motor will run opposite direction
     * 1 : Motor will run forward
     *
     * @param layer (Data8) LAYER – Specify chain layer number, [0 - 3]
     * @param nos (Data8) NOS – Output bit field, [0x00 – 0x0F]
     * @param polarity (Data8) POL – Polarity [-1, 0, 1]
     */
    public static EV3Operation opOutputPolarity(
            Layer layer,
            OutputPort[] nos,
            Polarity polarity) throws IOException {
        ByteArrayOutputStream outputStream = new ByteArrayOutputStream();
        LCEncoder.from(layer).lc0(outputStream);
        LCEncoder.from(nos).lc0(outputStream);
        LCEncoder.from(polarity.value()).lcx(outputStream);
        byte[] opData = outputStream.toByteArray();

        EV3OperationPackage opPackage = new EV3OperationPackage(OpCode.OUTPUT_POLARITY, opData);
        return new EV3Operation( opPackage);
    }

    /**
     * This function enables reading current motor speed and tacho count level.
     *
     * @param layer (Data8) LAYER – Specify chain layer number [0 - 3]
     * @param no (Data8) NO – Port number [0 - 3]
     * @param speed (Data8) *SPEED – Output speed level detected, [-100 - 100]
     * @param tacho (Data32) *TACHO – Current output tacho count
     */
    public static EV3Operation opOutputRead(
            Layer layer,
            OutputPort no,
            ReturnParam<Data8> speed,
            ReturnParam<Data32> tacho) throws IOException {
        ByteArrayOutputStream outputStream = new ByteArrayOutputStream();
        LCEncoder.from(layer).lc0(outputStream);
        LCEncoder.from(no).lc0(outputStream);
        GVEncoder.from(speed.getIndex()).gvx(outputStream);
        GVEncoder.from(tacho.getIndex()).gvx(outputStream);
        byte[] opData = outputStream.toByteArray();

        List<ReturnParam<? extends EV3Type>> paramList = Arrays.asList(speed, tacho);
        int gvSize = paramList.stream().mapToInt(ReturnParam::getSize).sum();

        EV3OperationPackage opPackage = new EV3OperationPackage(OpCode.OUTPUT_READ, opData, gvSize);
        return new EV3Operation( opPackage, paramList );
    }

    /**
     * This function enables the program to test if a output port is busy.
     *
     * @param layer (Data8) LAYER – Specify chain layer number [0 - 3]
     * @param nos (Data8) NOS – Output bit field [0x00 – 0x0F]
     * @param busy (Data8) BUSY – Output busy flag, [0 = Ready, 1 = Busy]
     */
    public static EV3Operation opOutputTest(
            Layer layer,
            OutputPort[] nos,
            ReturnParam<Data8> busy) throws IOException {
        ByteArrayOutputStream outputStream = new ByteArrayOutputStream();
        LCEncoder.from(layer).lc0(outputStream);
        LCEncoder.from(nos).lc0(outputStream);
        GVEncoder.from(busy.getIndex()).gvx(outputStream);
        byte[] opData = outputStream.toByteArray();

        EV3OperationPackage opPackage = new EV3OperationPackage(OpCode.OUTPUT_TEST, opData, busy.getSize());
        return new EV3Operation( opPackage, Arrays.asList(busy) );
    }

    /**
     * Enables program execution to wait for output ready. (Wait for completion)
     *
     * @param layer (Data8) LAYER – Specify chain layer number [0 - 3]
     * @param nos (Data8) NOS – Output bit field [0x00 – 0x0F]
     */
    public static EV3Operation opOutputReady(
            Layer layer,
            OutputPort[] nos) throws IOException {
        ByteArrayOutputStream outputStream = new ByteArrayOutputStream();
        LCEncoder.from(layer).lc0(outputStream);
        LCEncoder.from(nos).lc0(outputStream);
        byte[] opData = outputStream.toByteArray();

        EV3OperationPackage opPackage = new EV3OperationPackage(OpCode.OUTPUT_READY, opData);
        return new EV3Operation( opPackage );
    }

    /**
     * This function enables specifying a full motor power cycle in tacho counts. Step1
     * specifyes the power ramp up periode in tacho count, Step2 specifyes the constant
     * power period in tacho counts, Step 3 specifyes the power down period in tacho
     * counts.
     *
     * @param layer (Data8) LAYER – Specify chain layer number, [0 - 3]
     * @param nos (Data8) NOS – Output bit field, [0x00 – 0x0F]
     * @param power (Data8) POWER – Power level, [-100 - 100]
     * @param step1 (Data32) STEP1 – Tacho pulses during ramp up
     * @param step2 (Data32) STEP2 – Tacho pulses during continues run
     * @param step3 (Data32) STEP3 – Tacho pulses during ramp down
     * @param brake (Data8) BRAKE - Specify break level, [0: Float, 1: Break]
     */
    public static EV3Operation opOutputStepPower(
            Layer layer,
            OutputPort[] nos,
            Data8 power,
            Data32 step1,
            Data32 step2,
            Data32 step3,
            Brake brake) throws IOException {
        ByteArrayOutputStream outputStream = new ByteArrayOutputStream();
        LCEncoder.from(layer).lc0(outputStream);
        LCEncoder.from(nos).lc0(outputStream);
        LCEncoder.from(power).lcx(outputStream);
        LCEncoder.from(step1).lcx(outputStream);
        LCEncoder.from(step2).lcx(outputStream);
        LCEncoder.from(step3).lcx(outputStream);
        LCEncoder.from(brake.value()).lc0(outputStream);
        byte[] opData = outputStream.toByteArray();

        EV3OperationPackage opPackage = new EV3OperationPackage(OpCode.OUTPUT_STEP_POWER, opData);
        return new EV3Operation( opPackage );
    }

    /**
     * This function enables specifying a full motor power cycle in time. Step1 specifyes the
     * power ramp up periode in milliseconds, Step2 specifyes the constant power period in
     * milliseconds, Step 3 specifyes the power down period in milliseconds.
     *
     * @param layer (Data8) LAYER – Specify chain layer number [0 - 3]
     * @param nos (Data8) NOS – Output bit field [0x00 – 0x0F]
     * @param power (Data8) POWER – Power level, [-100 – 100]
     * @param step1 (Data32) STEP1 – Time in milliseconds for ramp up
     * @param step2 (Data32) STEP2 – Time in milliseconds for continues run
     * @param step3 (Data32) STEP3 – Time in milliseconds for ramp down
     * @param brake (Data8) BRAKE - Specify break level [0: Float, 1: Break]
     */
    public static EV3Operation opOutputTimePower(
            Layer layer,
            OutputPort[] nos,
            Data8 power,
            Data32 step1,
            Data32 step2,
            Data32 step3,
            Brake brake) throws IOException {
        ByteArrayOutputStream outputStream = new ByteArrayOutputStream();
        LCEncoder.from(layer).lc0(outputStream);
        LCEncoder.from(nos).lc0(outputStream);
        LCEncoder.from(power).lcx(outputStream);
        LCEncoder.from(step1).lcx(outputStream);
        LCEncoder.from(step2).lcx(outputStream);
        LCEncoder.from(step3).lcx(outputStream);
        LCEncoder.from(brake.value()).lc0(outputStream);
        byte[] opData = outputStream.toByteArray();

        EV3OperationPackage opPackage = new EV3OperationPackage(OpCode.OUTPUT_TIME_POWER, opData);
        return new EV3Operation( opPackage );
    }

    /**
     * This function enables specifying a full motor power cycle in tacho counts. The system
     * will automatically adjust the power level to the motor to keep the specified output
     * speed. Step1 specifyes the power ramp up periode in tacho count, Step2 specifyes
     * the constant power period in tacho counts, Step 3 specifyes the power down period
     * in tacho counts.
     *
     * @param layer (Data8) LAYER – Specify chain layer number, [0 - 3]
     * @param nos (Data8) NOS – Output bit field, [0x00 – 0x0F]
     * @param speed (Data8) SPEED – Power level, [-100 - 100]
     * @param step1 (Data32) STEP1 – Tacho pulses during ramp up
     * @param step2 (Data32) STEP2 – Tacho pulses during continues run
     * @param step3 (Data32) STEP3 – Tacho pulses during ramp down
     * @param brake (Data8) BRAKE - Specify break level, [0: Float, 1: Break]
     */
    public static EV3Operation opOutputStepSpeed(
            Layer layer,
            OutputPort[] nos,
            Data8 speed,
            Data32 step1,
            Data32 step2,
            Data32 step3,
            Brake brake) throws IOException {

        ByteArrayOutputStream outputStream = new ByteArrayOutputStream();

        LCEncoder.from(layer).lc0(outputStream);
        LCEncoder.from(nos).lc0(outputStream);
        LCEncoder.from(speed).lcx(outputStream);
        LCEncoder.from(step1).lcx(outputStream);
        LCEncoder.from(step2).lcx(outputStream);
        LCEncoder.from(step3).lcx(outputStream);
        LCEncoder.from(brake.value()).lc0(outputStream);
        byte[] opData = outputStream.toByteArray();

        EV3OperationPackage opPackage = new EV3OperationPackage(OpCode.OUTPUT_STEP_SPEED, opData);
        return new EV3Operation( opPackage);

    }

    /**
     * This function enables specifying a full motor power cycle in time. The system will
     * automatically adjust the power level to the motor to keep the specified output speed.
     * Step1 specifyes the power ramp up periode in milliseconds, Step2 specifyes the
     * constant power period in milliseconds, Step 3 specifyes the power down period in
     * milliseconds.
     *
     * @param layer (Data8) LAYER – Specify chain layer number [0 - 3]
     * @param nos (Data8) NOS – Output bit field [0x00 – 0x0F]
     * @param speed (Data8) SPEED – Power level, [-100 – 100]
     * @param step1 (Data32) STEP1 – Time in milliseconds for ramp up
     * @param step2 (Data32) STEP2 – Time in milliseconds for continues run
     * @param step3 (Data32) STEP3 – Time in milliseconds for ramp down
     * @param brake (Data8) BRAKE - Specify break level [0: Float, 1: Break]
     * @return
     */
    public static EV3Operation opOutputTimeSpeed(
            Layer layer,
            OutputPort[] nos,
            Data8 speed,
            Data32 step1,
            Data32 step2,
            Data32 step3,
            Brake brake) throws IOException {
        ByteArrayOutputStream outputStream = new ByteArrayOutputStream();

        LCEncoder.from(layer).lc0(outputStream);
        LCEncoder.from(nos).lc0(outputStream);
        LCEncoder.from(speed).lcx(outputStream);
        LCEncoder.from(step1).lcx(outputStream);
        LCEncoder.from(step2).lcx(outputStream);
        LCEncoder.from(step3).lcx(outputStream);
        LCEncoder.from(brake.value()).lc0(outputStream);
        byte[] opData = outputStream.toByteArray();

        EV3OperationPackage opPackage = new EV3OperationPackage(OpCode.OUTPUT_TIME_SPEED, opData);
        return new EV3Operation( opPackage);
    }

    /**
     * This function enables synchonizing two motors. Synchonization should be used when
     * motors should run as synchrone as possible, for example to archieve a model driving
     * straight. Duration is specified in tacho counts.
     *
     * Turn ratio:
     * 0 : Motor will run with same power
     * 100 : One motor will run with specified power while the other will be close to zero
     * 200: One motor will run with specified power forward while the other will run in the
     * opposite direction at the same power level.
     *
     * @param layer (Data8) LAYER – Specify chain layer number [0 - 3]
     * @param nos (Data8) NOS – Output bit field [0x00 – 0x0F]
     * @param speed (Data8) SPEED – Power level, [-100 – 100]
     * @param turn (Data16) TURN – Turn ratio, [-200 - 200], see documentation below
     * @param step (Data32) STEP – Tacho pulses, 0 = Infinite
     * @param brake (Data8) BRAKE - Specify break level [0: Float, 1: Break]
     * @return
     */
    public static EV3Operation opOutputStepSync(
            Layer layer,
            OutputPort[] nos,
            Data8 speed,
            Data16 turn,
            Data32 step,
            Brake brake) throws IOException {
        ByteArrayOutputStream outputStream = new ByteArrayOutputStream();

        LCEncoder.from(layer).lc0(outputStream);
        LCEncoder.from(nos).lc0(outputStream);
        LCEncoder.from(speed).lcx(outputStream);
        LCEncoder.from(turn).lcx(outputStream);
        LCEncoder.from(step).lcx(outputStream);
        LCEncoder.from(brake.value()).lc0(outputStream);
        byte[] opData = outputStream.toByteArray();

        EV3OperationPackage opPackage = new EV3OperationPackage(OpCode.OUTPUT_STEP_SYNC, opData);
        return new EV3Operation( opPackage);
    }

    /**
     * This function enables synchonizing two motors. Synchonization should be used when
     * motors should run as synchrone as possible, for example to archieve a model driving
     * straight. Duration is specified in time.
     *
     * @param layer (Data8) LAYER – Specify chain layer number [0 - 3]
     * @param nos (Data8) NOS – Output bit field [0x00 – 0x0F]
     * @param speed (Data8) SPEED – Power level, [-100 – 100]
     * @param turn (Data16) TURN – Turn ratio, [-200 - 200], see documentation below
     * @param time (Data32) TIME – Time in milliseconds, 0 = Infinite
     * @param brake (Data8) BRAKE - Specify break level [0: Float, 1: Break]
     * @return
     */
    public static EV3Operation opOutputTimeSync(
            Layer layer,
            OutputPort[] nos,
            Data8 speed,
            Data16 turn,
            Data32 time,
            Brake brake) throws IOException {
        ByteArrayOutputStream outputStream = new ByteArrayOutputStream();

        LCEncoder.from(layer).lc0(outputStream);
        LCEncoder.from(nos).lc0(outputStream);
        LCEncoder.from(speed).lcx(outputStream);
        LCEncoder.from(turn).lcx(outputStream);
        LCEncoder.from(time).lcx(outputStream);
        LCEncoder.from(brake.value()).lc0(outputStream);
        byte[] opData = outputStream.toByteArray();

        EV3OperationPackage opPackage = new EV3OperationPackage(OpCode.OUTPUT_TIME_SYNC, opData);
        return new EV3Operation( opPackage);
    }

    /**
     * This function enables the program to clear the tacho count used as sensor input.
     *
     * @param layer (Data8) LAYER – Specify chain layer number [0 - 3]
     * @param nos (Data8) NOS – Output bit field [0x00 – 0x0F]
     */
    public static EV3Operation opOutputClrCount(
            Layer layer,
            OutputPort[] nos) throws IOException {
        ByteArrayOutputStream outputStream = new ByteArrayOutputStream();

        LCEncoder.from(layer).lc0(outputStream);
        LCEncoder.from(nos).lc0(outputStream);
        byte[] opData = outputStream.toByteArray();

        EV3OperationPackage opPackage = new EV3OperationPackage(OpCode.OUTPUT_CLR_COUNT, opData);
        return new EV3Operation( opPackage);
    }

    /**
     * This function enables the program to read the tacho count as sensor input.
     *
     * @param layer (Data8) LAYER – Specify chain layer number [0 - 3]
     * @param nos (Data8) NOS – Output bit field [0x00 – 0x0F]
     * @param tacho (Data32) *TACHO – Tacho count as sensor value
     */
    public static EV3Operation opOutputGetCount(
            Layer layer,
            OutputPort[] nos,
            ReturnParam<Data32> tacho) throws IOException {
        ByteArrayOutputStream outputStream = new ByteArrayOutputStream();

        LCEncoder.from(layer).lc0(outputStream);
        LCEncoder.from(nos).lc0(outputStream);
        GVEncoder.from(tacho.getIndex()).gvx(outputStream);
        byte[] opData = outputStream.toByteArray();

        EV3OperationPackage opPackage = new EV3OperationPackage(OpCode.OUTPUT_GET_COUNT, opData);
        return new EV3Operation( opPackage);
    }

    /**
     * This function should be called a program end. It enables breaking the motor for a
     * short period and right after floating the motors. The function relates to the layer on
     * which it is executed.
     */
    public static EV3Operation opOutputPrgStop() {
        EV3OperationPackage opPackage = new EV3OperationPackage(OpCode.OUTPUT_PRG_STOP, null);
        return new EV3Operation( opPackage);
    }

}
